#ifndef DIFFERENTIAL_DRIVE_H
#define DIFFERENTIAL_DRIVE_H

#include "motor_controller.h"

// 两轮差速驱动实现
class DifferentialDrive : public MotorController {
private:
  int motorL1, motorL2, motorR1, motorR2;
  int maxSpeed;

public:
  DifferentialDrive(int l1, int l2, int r1, int r2) {
    motorL1 = l1;
    motorL2 = l2;
    motorR1 = r1;
    motorR2 = r2;
    maxSpeed = 255;
  }

  void init() override {
    pinMode(motorL1, OUTPUT);
    pinMode(motorL2, OUTPUT);
    pinMode(motorR1, OUTPUT);
    pinMode(motorR2, OUTPUT);
    stop();
  }

  void forward(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    analogWrite(motorL1, speed);
    analogWrite(motorL2, 0);
    analogWrite(motorR1, speed);
    analogWrite(motorR2, 0);
  }

  void backward(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    analogWrite(motorL1, 0);
    analogWrite(motorL2, speed);
    analogWrite(motorR1, 0);
    analogWrite(motorR2, speed);
  }

  void turnLeft(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    analogWrite(motorL1, 0);
    analogWrite(motorL2, speed);
    analogWrite(motorR1, speed);
    analogWrite(motorR2, 0);
  }

  void turnRight(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    analogWrite(motorL1, speed);
    analogWrite(motorL2, 0);
    analogWrite(motorR1, 0);
    analogWrite(motorR2, speed);
  }

  void stop() override {
    analogWrite(motorL1, 0);
    analogWrite(motorL2, 0);
    analogWrite(motorR1, 0);
    analogWrite(motorR2, 0);
  }

  void setSpeed(int speed) override {
    maxSpeed = constrain(speed, 0, 255);
  }

  String getType() override {
    return "DifferentialDrive";
  }
};

#endif // DIFFERENTIAL_DRIVE_H